#include <ros/ros.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tf.h>

struct Position
{
    double x;   // m
    double y;   // m
    double yaw; // deg
};

#define pi 3.14

class Line
{
public:
    Line();

private:
    void callBack(const nav_msgs::Odometry::ConstPtr &odom);
    ros::NodeHandle n;
    ros::Publisher pub;
    ros::Subscriber sub;
};

Line::Line()
{
    pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
    sub = n.subscribe<nav_msgs::Odometry>("odom", 1000, &Line::callBack, this);
}

void Line::callBack(const nav_msgs::Odometry::ConstPtr &odom)
{
    geometry_msgs::Twist cmd;

    Position current_pose;
    current_pose.x = odom->pose.pose.position.x;
    current_pose.y = odom->pose.pose.position.y;

    tf::Quaternion quat;
    tf::quaternionMsgToTF(odom->pose.pose.orientation, quat);
    double roll, pitch, yaw;                      //定义存储r\p\y的容器
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); //进行转换
    current_pose.yaw = yaw;

    /*puresuit*/
    double y_target[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    double x_target[] = {-1.2,-0.7,-0.4,-0.1,0.2,0.5,0.8,1.1,1.4,1.7,2,2.3,2.6,2.9,3.2,3.5,3.8,4.1,4.4,4.7,5};
    int n = sizeof(y_target) / sizeof(double);
    double z[n] = {}; //z数组存放小车当前位置到直线各点距离值
    for (int i = 0; i < n; i++)
    {
        z[i] = sqrt((current_pose.x - x_target[i]) * (current_pose.x - x_target[i]) + (current_pose.y - y_target[i]) * (current_pose.y - y_target[i]));
    }

    int num = std::distance(z, std::min_element(z, z + sizeof(z) / sizeof(z[0]))); //求直线离小车最近点位置
    double nearestPosX = x_target[num];                                  //（nearePosX，nearePosY）为直线离小车最近点
    double nearestPosY = y_target[num];

    //找（num,n）区间预瞄点
    double d1[n] = {};
    double r1[n] = {};
    for (int j = 0; j < n - num; j++)
    {
        d1[j] = sqrt((nearestPosX - x_target[j + num + 1]) * (nearestPosX - x_target[j + num + 1]) + (nearestPosY - y_target[j + num + 1]) * (nearestPosY - y_target[j + num + 1]));
        r1[j] = fabs(d1[j] - 2); //以（nearePosX，nearePosY）为圆心，半径2画圆，求交点为预瞄点
    }
    int num_r1 = std::distance(r1, std::min_element(r1, r1 + sizeof(r1) / sizeof(r1[0])));
    //cout << num_r1+num+1 <<endl;    //预瞄点在x_target[]位置；

    //找（0,num）区间预瞄点
    double d2[n] = {};
    double r2[n] = {};
    for (int k = 0; k < num; k++)
    {
        d2[k] = sqrt((nearestPosX - x_target[k]) * (nearestPosX - x_target[k]) + (nearestPosY - y_target[k]) * (nearestPosY - y_target[k]));
        r2[k] = fabs(d2[k] - 2);
    }
    int num_r2 = std::distance(r2, std::min_element(r2, r2 + sizeof(r2) / sizeof(r2[0])));
    //cout << num_r2 <<endl;  // 预瞄点在x_target[],y_target[]位置 ，num_r2;

    //从两个预瞄点中选出小车行进方向预瞄点
    double v_r1[4] = {nearestPosX, nearestPosY, x_target[num_r1 + num + 1], y_target[num_r1 + num + 1]};
    double v_r2[4] = {nearestPosX, nearestPosY, x_target[num_r2], y_target[num_r2]};
    double v_yaw[4] = {0, 0, cos(yaw), sin(yaw)};
    double yumiao_x = 0;
    double yumiao_y = 0;

    double t1 = ((v_r1[0] - v_r1[2]) * (v_yaw[0] - v_yaw[2]) + (v_r1[1] - v_r1[3]) * (v_yaw[1] - v_yaw[3])) / (sqrt(pow(v_r1[0] - v_r1[2], 2) + pow(v_r1[1] - v_r1[3], 2)) * sqrt(pow(v_yaw[0] - v_yaw[2], 2) + pow(v_yaw[1] - v_yaw[3], 2)));
    double angle_1 = acos(t1) * (180 / pi);

    double t2 = ((v_r2[0] - v_r2[2]) * (v_yaw[0] - v_yaw[2]) + (v_r2[1] - v_r2[3]) * (v_yaw[1] - v_yaw[3])) / (sqrt(pow(v_r2[0] - v_r2[2], 2) + pow(v_r2[1] - v_r2[3], 2)) * sqrt(pow(v_yaw[0] - v_yaw[2], 2) + pow(v_yaw[1] - v_yaw[3], 2)));
    double angle_2 = acos(t2) * (180 / pi);

    //cout <<"angle_1 "<<angle_1 <<endl <<"angle_2 "<<angle_2 <<endl;

    double next_angle, angle_m;
    double *p = &next_angle; //指向预瞄点
    double *q = &angle_m;    //求预瞄点切线角度
    if (fabs(angle_1) < 90)
    {
        yumiao_x = x_target[num_r1 + num + 1];
        yumiao_y = y_target[num_r1 + num + 1];
        double v_3[4] = {current_pose.x,current_pose.y, yumiao_x, yumiao_y};
        double t3 = ((v_3[0] - v_3[2]) * (v_yaw[0] - v_yaw[2]) + (v_3[1] - v_3[3]) * (v_yaw[1] - v_yaw[3])) / (sqrt(pow(v_3[0] - v_3[2], 2) + pow(v_3[1] - v_3[3], 2)) * sqrt(pow(v_yaw[0] - v_yaw[2], 2) + pow(v_yaw[1] - v_yaw[3], 2)));
        *p = acos(t3);
        *q = atan((y_target[num_r1 + num + 2] - y_target[num_r1 + num + 1]) / (x_target[num_r1 + num + 2] - x_target[num_r1 + num + 1]));
    }
    else if (fabs(angle_2) < 90)
    {
        yumiao_x = x_target[num_r2];
        yumiao_y = y_target[num_r2];
        double v_4[4] = {current_pose.x, current_pose.y, yumiao_x, yumiao_y};
        double t4 = ((v_4[0] - v_4[2]) * (v_yaw[0] - v_yaw[2]) + (v_4[1] - v_4[3]) * (v_yaw[1] - v_yaw[3])) / (sqrt(pow(v_4[0] - v_4[2], 2) + pow(v_4[1] - v_4[3], 2)) * sqrt(pow(v_yaw[0] - v_yaw[2], 2) + pow(v_yaw[1] - v_yaw[3], 2)));
        *p = acos(t4);
        *q = atan((y_target[num_r2 + 1] - y_target[num_r2]) / (x_target[num_r2 + 1] - x_target[num_r2]));
    }

    /*cout << "pose " <<yumiao_x <<"/"<<yumiao_y <<endl;
         cout << "next_angle " <<next_angle <<endl;
         cout << "angle_m "<<angle_m <<endl;*/

    //front_wheel_steer
    double L = 0.35;
    double l = sqrt(pow(yumiao_x - current_pose.x, 2) + pow(yumiao_y - current_pose.y, 2));
    double steer1 = atan(2 * L * sin(next_angle) / l);

    /*航向角PI控制
        double    Kp=0.5;
        double    Ki=0.2;
        double  integral=0;
        double    steer2=0;
        for (int t=0;t<6;t++)
        {
          double error_angle=angle_m-yaw;
          steer2=Kp*error_angle+Ki*integral;
          integral=integral+error_angle;
          cout << steer2<<endl;
        }
        */
    double steer2 = yaw - angle_m;
    //double  steer_H=0.6*steer1+0.4*steer2;
    double steer_L = 0.2 * steer1 + 0.8 * steer2;

    /*cout << "steer1 "<<steer1 <<endl
             <<"steer2 "<<steer2<<endl
             <<"steerL "<<steer_L<<endl;*/
    double R = L / tan(steer_L);
    double D = 0.6;
    double V = 0.1, W = 0;

    if (steer_L < 0) // turn Left
    {
        W = V / (R + D / 2);
    }
    else // turn Right
    {
        W = -V / (R + D / 2);
    }

    cmd.linear.x = V;
    cmd.angular.z = W;
    pub.publish(cmd);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "line_steer");
    Line line;
    ros::spin();
}